#include "ofc_ring_buf.h"
#include "bsp_laser_stm32f103.h"
#include "bsp_laser.h"
#include "bsp_laser_def.h"
#include "wit_c_sdk.h"
#include "REG.h"

static UInt8 Bsp_CalcXor(const UInt8* data, Int32 count, UInt8 start)
{
    const UInt8* data_end = data + count;

	for (; data < data_end; data++)
	{
	    start = start ^ *data;
	}

	return start;
}

static void Bsp_LaserStateMachine_(Bsp_TLaserAttr* self, UInt8* buf, Int32 count)
{
    static UInt8 recv[8];
    static UInt8 state = 0;

    for (UInt8* buf_end = buf + count; buf < buf_end; buf++)
    {
        if (state == 0)
        {
            /// 帧头
            if (*buf == 0x7B)
                state = 1;
        }
        else if (state == 1)
        {
            recv[0] = *buf;
            state = 2;
        }
        else if (state == 2)
        {
            recv[1] = *buf;
            state = 3;
        }
        else if (state == 3)
        {
            recv[2] = *buf;
            state = 4;
        }
        else if (state == 4)
        {
            recv[3] = *buf;
            state = 5;
        }
        else if (state == 5)
        {
            recv[4] = *buf;
            state = 6;
        }
        else if (state == 6)
        {
            recv[5] = *buf;
            state = 7;
        }
        else if (state == 7)
        {
            recv[6] = *buf;
            state = 8;
        }
        else if (state == 8)
        {
            recv[7] = *buf;
            state = 9;
        }
        else if (state == 9)
        {
            if (Bsp_CalcXor((const UInt8*)&recv, sizeof(recv), 0x7B) == *buf)
                state = 10;
            else
                state = 0;
        }
        else if (state == 10)
        {
            /// 帧尾
            if (*buf == 0x7D)
                state = 11;
            else
                state = 0;
        }
        
        if (state == 11)
        {
            Bsp_TLaserData data;
            data.distance1 = (recv[0] << 8) | recv[1];
            data.distance2 = (recv[2] << 8) | recv[3];
            data.distance3 = (recv[4] << 8) | recv[5];
            data.distance4 = (recv[6] << 8) | recv[7];

            __disable_irq();
            if (TOfRingBufWrite(&self->ring_buf, &data, sizeof(data)) != sizeof(data))
            {
                TOfRingBufPop(&self->ring_buf, sizeof(data));
                TOfRingBufWrite(&self->ring_buf, &data, sizeof(data));
            }
            __enable_irq();

            state = 0;
        }
    }
}

Bsp_THandle Bsp_LaserCreate(Int32 id)
{
    Bsp_THandle result = NULL;
    Bsp_TLaserAttr* self = &g_bsp_laser_attr;

    if (id >= Bsp_kLaserIdBase && id < Bsp_kLaserIdMax)
    {
        self->data = NULL;
        self->depth = 0;
        self->is_open = False;

        self->laser_handle = Bsp_UartCreate(self->laser_id, self->send_buf, self->send_buf_size, self->recv_buf, self->recv_buf_size);
        if (self->laser_handle != NULL)
        {
            result = (Bsp_THandle)self;
        }
    }

    return result;
}

void Bsp_LaserFree(Bsp_THandle handle)
{
    Bsp_TLaserAttr* self = &g_bsp_laser_attr;
    if (handle == self)
    {
        Bsp_LaserClose(handle);

        Bsp_UartFree(self->laser_handle);
    }
}

Int32 Bsp_LaserOpen(Bsp_THandle handle)
{
    Bsp_TLaserAttr* self = &g_bsp_laser_attr;
    Int32 result = Bsp_kErrorCodeParam;

    if (handle == self)
    {
        if (!self->is_open)
        {
            if (self->data == NULL)
                result = Bsp_kErrorCodeProcess;
            else
            {
                TOfRingBufCreate(&self->ring_buf, self->data, self->depth * sizeof(*(self->data)));

                if (Bsp_UartOpen(self->laser_handle, 115200, 0) >= 0)
                {
                    self->is_open = True;
                    result = Bsp_kErrorCodeSuccess;
                }
                else
                    result = Bsp_kErrorCodeFailed;
            }
        }
        else
            result = Bsp_kErrorCodeOpened;
    }

    return result;
}

Int32 Bsp_LaserClose(Bsp_THandle handle)
{
    Bsp_TLaserAttr* self = &g_bsp_laser_attr;
    Int32 result = Bsp_kErrorCodeParam;

    if (handle == self)
    {
        if (self->is_open)
        {
            if (Bsp_UartClose(self->laser_handle) >= 0)
            {
                self->is_open = False;
                result = Bsp_kErrorCodeSuccess;
            }
            else
                result = Bsp_kErrorCodeFailed;
        }
        else
            result = Bsp_kErrorCodeClosed;
    }

    return result;
}

Int32 Bsp_LaserSetDataDepth(Bsp_THandle handle, Int8 data_type, void* data, Int32 depth)
{
    Bsp_TLaserAttr* self = &g_bsp_laser_attr;
    Int32 result = Bsp_kErrorCodeParam;

    if (handle == self && (UInt8)data_type < Bsp_kLaserDataTypeMax && data != NULL && depth > 0)
    {
        if (!self->is_open)
        {
            if (data_type == Bsp_kLaserDataTypeCollectBoard)
            {
                self->data = data;
                self->depth = depth;
                result = Bsp_kErrorCodeSuccess;
            }
            else
                result = Bsp_kErrorCodeNotSupport;
        }
        else
            result = Bsp_kErrorCodeOpened;
    }

    return result;
}

Int32 Bsp_LaserGetData(Bsp_THandle handle, Int8 data_type, void* data, Int32 count)
{
    Bsp_TLaserAttr* self = &g_bsp_laser_attr;
    Int32 result = Bsp_kErrorCodeParam;

    if (handle == self && (UInt8)data_type < Bsp_kLaserDataTypeMax && data != NULL && count > 0)
    {
        if (self->is_open)
        {
            if (data_type == Bsp_kLaserDataTypeCollectBoard)
            {
                if (self->data != NULL)
                {
                    result = TOfRingBufRead(&self->ring_buf, data, count * sizeof(*(self->data)));
                    result = result / sizeof(*(self->data));
                }
                else
                    result = Bsp_kErrorCodeProcess;
            }
            else
                result = Bsp_kErrorCodeNotSupport;
        }
        else
            result = Bsp_kErrorCodeClosed;
    }

    return result;
}

Int32 Bsp_LaserRunOnce(Bsp_THandle handle)
{
    Bsp_TLaserAttr* self = &g_bsp_laser_attr;
    Int32 result = Bsp_kErrorCodeParam;

    if (handle == self)
    {
        if (self->is_open)
        {
            Int32 count;
            // 读取数据
            count = Bsp_UartRead(self->laser_handle, self->read_buf, self->read_buf_size);
            if (count > 0 && self->data != NULL)
                Bsp_LaserStateMachine_(self, self->read_buf, count);

            result = Bsp_kErrorCodeSuccess;
        }
        else
            result = Bsp_kErrorCodeClosed;
    }

    return result;
}
